/*
 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.box2dflash.dynamics.joints {
	import org.box2dflash.dynamics.joints.Jacobian;
	import org.box2dflash.dynamics.Body;
	import org.box2dflash.common.math.Mat22;
	import org.box2dflash.common.*;
	import org.box2dflash.common.math.*;
	import org.box2dflash.dynamics.*;
	use namespace Internal;

	// Linear constraint (point-to-line)
	// d = p2 - p1 = x2 + r2 - x1 - r1
	// C = dot(ay1, d)
	// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
	//      = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
	// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
	//
	// Angular constraint
	// C = a2 - a1 + a_initial
	// Cdot = w2 - w1
	// J = [0 0 -1 0 0 1]

	// Motor/Limit linear constraint
	// C = dot(ax1, d)
	// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
	// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
	public class PrismaticJoint extends Joint {
		public override function get anchor1():Vec2 {
			return m_body1.worldPoint(m_localAnchor1);
		}

		public override function get anchor2():Vec2 {
			return m_body2.worldPoint(m_localAnchor2);
		}

		public override function get reactionForce():Vec2 {
			var tMat:Mat22 = m_body1.m_xf.R;
			//Vec2 ax1 = Mul(m_body1->m_xf.R, m_localXAxis1);
			var ax1X:Number = m_limitForce * (tMat.col1.x * m_localXAxis1.x + tMat.col2.x * m_localXAxis1.y);
			var ax1Y:Number = m_limitForce * (tMat.col1.y * m_localXAxis1.x + tMat.col2.y * m_localXAxis1.y);
			//Vec2 ay1 = Mul(m_body1->m_xf.R, m_localYAxis1);
			var ay1X:Number = m_force * (tMat.col1.x * m_localYAxis1.x + tMat.col2.x * m_localYAxis1.y);
			var ay1Y:Number = m_force * (tMat.col1.y * m_localYAxis1.x + tMat.col2.y * m_localYAxis1.y);
		
			//return m_limitForce * ax1 + m_force * ay1;
			return new Vec2(m_limitForce * ax1X + m_force * ay1X, m_limitForce * ax1Y + m_force * ay1Y);
		}

		public override function get reactionTorque():Number {
			return m_torque;
		}

		/// Get the current joint translation, usually in meters.
		public function get jointTranslation():Number {
			var b1:Body = m_body1;
			var b2:Body = m_body2;
		
		//NEVER USED
//			var tMat:Mat22;
		
			var p1:Vec2 = b1.worldPoint(m_localAnchor1);
			var p2:Vec2 = b2.worldPoint(m_localAnchor2);
			//var d:Vec2 = Box2dMath.SubtractVV(p2, p1);
			var dX:Number = p2.x - p1.x;
			var dY:Number = p2.y - p1.y;
			//Vec2 axis = b1->GetWorldVector(m_localXAxis1);
			var axis:Vec2 = b1.worldVector(m_localXAxis1);
		
			//float32 translation = Dot(d, axis);
			var translation:Number = axis.x * dX + axis.y * dY;
			return translation;
		}

		/// Get the current joint translation speed, usually in meters per second.
		public function get jointSpeed():Number {
			var b1:Body = m_body1;
			var b2:Body = m_body2;
		
			var tMat:Mat22;
		
			//Vec2 r1 = Mul(b1->m_xf.R, m_localAnchor1 - b1->GetLocalCenter());
			tMat = b1.m_xf.R;
			var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
			var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
			var tX:Number = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
			r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
			r1X = tX;
			//Vec2 r2 = Mul(->m_xf.R, m_localAnchor2 - ->GetLocalCenter());
			tMat = b2.m_xf.R;
			var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
			var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
			tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
			r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
			r2X = tX;
		
			//Vec2 p1 = b1->m_sweep.c + r1;
			var p1X:Number = b1.m_sweep.c.x + r1X;
			var p1Y:Number = b1.m_sweep.c.y + r1Y;
			//Vec2 p2 = ->m_sweep.c + r2;
			var p2X:Number = b2.m_sweep.c.x + r2X;
			var p2Y:Number = b2.m_sweep.c.y + r2Y;
			//var d:Vec2 = Box2dMath.SubtractVV(p2, p1);
			var dX:Number = p2X - p1X;
			var dY:Number = p2Y - p1Y;
			//Vec2 axis = b1->GetWorldVector(m_localXAxis1);
			var axis:Vec2 = b1.worldVector(m_localXAxis1);
		
			var v1:Vec2 = b1.m_linearVelocity;
			var v2:Vec2 = b2.m_linearVelocity;
			var w1:Number = b1.m_angularVelocity;
			var w2:Number = b2.m_angularVelocity;
		
			//var speed:Number = Box2dMath.Dot(d, Box2dMath.CrossFV(w1, ax1)) + Box2dMath.Dot(ax1, Box2dMath.SubtractVV( Box2dMath.SubtractVV( Box2dMath.AddVV( v2 , Box2dMath.CrossFV(w2, r2)) , v1) , Box2dMath.CrossFV(w1, r1)));
			//var D:Number = (dX*(-w1 * ax1Y) + dY*(w1 * ax1X));
			//var D2:Number = (ax1X * ((( v2.x + (-w2 * r2Y)) - v1.x) - (-w1 * r1Y)) + ax1Y * ((( v2.y + (w2 * r2X)) - v1.y) - (w1 * r1X)));
			var speed:Number = (dX * (-w1 * axis.y) + dY * (w1 * axis.x)) + (axis.x * ((( v2.x + (-w2 * r2Y)) - v1.x) - (-w1 * r1Y)) + axis.y * ((( v2.y + (w2 * r2X)) - v1.y) - (w1 * r1X)));
		
			return speed;
		}

		/// Is the joint limit enabled?
		public function get isLimitEnabled():Boolean {
			return m_enableLimit;
		}

		/// Enable/disable the joint limit.
		public function enableLimit(flag:Boolean):void {
			m_enableLimit = flag;
		}

		/// Get the lower joint limit, usually in meters.
		public function get lowerLimit():Number {
			return m_lowerTranslation;
		}

		/// Get the upper joint limit, usually in meters.
		public function get upperLimit():Number {
			return m_upperTranslation;
		}

		/// Set the joint limits, usually in meters.
		public function limits(lower:Number, upper:Number):void {
			//Settings.Assert(lower <= upper);
			m_lowerTranslation = lower;
			m_upperTranslation = upper;
		}

		/// Is the joint motor enabled?
		public function get isMotorEnabled():Boolean {
			return m_enableMotor;
		}

		/// Enable/disable the joint motor.
		public function enableMotor(flag:Boolean):void {
			m_enableMotor = flag;
		}

		/// Set the motor speed, usually in meters per second.
		public function set motorSpeed(speed:Number):void {
			m_motorSpeed = speed;
		}

		/// Get the motor speed, usually in meters per second.
		public function get motorSpeed():Number {
			return m_motorSpeed;
		}

		/// Set the maximum motor force, usually in N.
		public function set maxMotorForce(force:Number):void {
			m_maxMotorForce = force;
		}

		/// Get the current motor force, usually in N.
		public function get motorForce():Number {
			return m_motorForce;
		}

		//--------------- Internals Below -------------------
		public function PrismaticJoint(def:PrismaticJointDef) {
			super(def);
		//NEVER USED
//			var tMat:Mat22;
//			var tX:Number;
//			var tY:Number;
		
			m_localAnchor1.v(def.localAnchor1);
			m_localAnchor2.v(def.localAnchor2);
			m_localXAxis1.v(def.localAxis1);
		
			//m_localYAxis1 = Cross(1.0f, m_localXAxis1);
			m_localYAxis1.x = -m_localXAxis1.y;
			m_localYAxis1.y = m_localXAxis1.x;
		
			m_refAngle = def.referenceAngle;
		
			m_linearJacobian.zero();
			m_linearMass = 0.0;
			m_force = 0.0;
		
			m_angularMass = 0.0;
			m_torque = 0.0;
		
			m_motorJacobian.zero();
			m_motorMass = 0.0;
			m_motorForce = 0.0;
			m_limitForce = 0.0;
			m_limitPositionImpulse = 0.0;
		
			m_lowerTranslation = def.lowerTranslation;
			m_upperTranslation = def.upperTranslation;
			m_maxMotorForce = def.maxMotorForce;
			m_motorSpeed = def.motorSpeed;
			m_enableLimit = def.enableLimit;
			m_enableMotor = def.enableMotor;
		}

		Internal override function initVelocityConstraints(step:TimeStep):void {
			var b1:Body = m_body1;
			var b2:Body = m_body2;
		
			var tMat:Mat22;
			var tX:Number;
		
			// Compute the effective masses.
			//Vec2 r1 = Mul(b1->m_xf.R, m_localAnchor1 - b1->GetLocalCenter());
			tMat = b1.m_xf.R;
			var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
			var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
			tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
			r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
			r1X = tX;
			//Vec2 r2 = Mul(->m_xf.R, m_localAnchor2 - ->GetLocalCenter());
			tMat = b2.m_xf.R;
			var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
			var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
			tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
			r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
			r2X = tX;
		
			//float32 invMass1 = b1->m_invMass, invMass2 = ->m_invMass;
			var invMass1:Number = b1.m_invMass;
			var invMass2:Number = b2.m_invMass;
			//float32 invI1 = b1->m_invI, invI2 = ->m_invI;
			var invI1:Number = b1.m_invI;
			var invI2:Number = b2.m_invI;
		
			// Compute point to line constraint effective mass.
			// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
			//Vec2 ay1 = Mul(b1->m_xf.R, m_localYAxis1);
			tMat = b1.m_xf.R;
			var ay1X:Number = tMat.col1.x * m_localYAxis1.x + tMat.col2.x * m_localYAxis1.y;
			var ay1Y:Number = tMat.col1.y * m_localYAxis1.x + tMat.col2.y * m_localYAxis1.y;
			//Vec2 e = ->m_sweep.c + r2 - b1->m_sweep.c;	// e = d + r1
			var eX:Number = b2.m_sweep.c.x + r2X - b1.m_sweep.c.x;
			var eY:Number = b2.m_sweep.c.y + r2Y - b1.m_sweep.c.y;
		
			//m_linearJacobian.Set(-ay1, -Box2dMath.Cross(e, ay1), ay1, Box2dMath.Cross(r2, ay1));
			m_linearJacobian.linear1.x = -ay1X; 
			m_linearJacobian.linear1.y = -ay1Y;
			m_linearJacobian.linear2.x = ay1X; 
			m_linearJacobian.linear2.y = ay1Y;
			m_linearJacobian.angular1 = -(eX * ay1Y - eY * ay1X); 
			m_linearJacobian.angular2 = r2X * ay1Y - r2Y * ay1X; 
		
			m_linearMass = invMass1 + invI1 * m_linearJacobian.angular1 * m_linearJacobian.angular1 + invMass2 + invI2 * m_linearJacobian.angular2 * m_linearJacobian.angular2;
			//Settings.Assert(m_linearMass > Number.MIN_VALUE);
			m_linearMass = 1.0 / m_linearMass;
		
			// Compute angular constraint effective mass.
			m_angularMass = invI1 + invI2;
			if (m_angularMass > Number.MIN_VALUE) {
				m_angularMass = 1.0 / m_angularMass;
			}
		
			// Compute motor and limit terms.
			if (m_enableLimit || m_enableMotor) {
				// The motor and limit share a Jacobian and effective mass.
				//Vec2 ax1 = Mul(b1->m_xf.R, m_localXAxis1);
				tMat = b1.m_xf.R;
				var ax1X:Number = tMat.col1.x * m_localXAxis1.x + tMat.col2.x * m_localXAxis1.y;
				var ax1Y:Number = tMat.col1.y * m_localXAxis1.x + tMat.col2.y * m_localXAxis1.y;
				//m_motorJacobian.Set(-ax1, -Cross(e, ax1), ax1, Cross(r2, ax1));
				m_motorJacobian.linear1.x = -ax1X; 
				m_motorJacobian.linear1.y = -ax1Y;
				m_motorJacobian.linear2.x = ax1X; 
				m_motorJacobian.linear2.y = ax1Y;
				m_motorJacobian.angular1 = -(eX * ax1Y - eY * ax1X); 
				m_motorJacobian.angular2 = r2X * ax1Y - r2Y * ax1X; 
			
				m_motorMass = invMass1 + invI1 * m_motorJacobian.angular1 * m_motorJacobian.angular1 + invMass2 + invI2 * m_motorJacobian.angular2 * m_motorJacobian.angular2;
				//Settings.Assert(m_motorMass > Number.MIN_VALUE);
				m_motorMass = 1.0 / m_motorMass;
			
				if (m_enableLimit) {
					//Vec2 d = e - r1;	// p2 - p1
					var dX:Number = eX - r1X;
					var dY:Number = eY - r1Y;
					//float32 jointTranslation = Dot(ax1, d);
					var jointTranslation:Number = ax1X * dX + ax1Y * dY;
					if (Box2dMath.abs(m_upperTranslation - m_lowerTranslation) < 2.0 * Settings._linearSlop) {
						m_limitState = e_equalLimits;
					}
				else if (jointTranslation <= m_lowerTranslation) {
						if (m_limitState != e_atLowerLimit) {
							m_limitForce = 0.0;
						}
						m_limitState = e_atLowerLimit;
					}
				else if (jointTranslation >= m_upperTranslation) {
						if (m_limitState != e_atUpperLimit) {
							m_limitForce = 0.0;
						}
						m_limitState = e_atUpperLimit;
					}
				else {
						m_limitState = e_inactiveLimit;
						m_limitForce = 0.0;
					}
				}
			}
		
			if (m_enableMotor == false) {
				m_motorForce = 0.0;
			}
		
			if (m_enableLimit == false) {
				m_limitForce = 0.0;
			}
		
			if (step.warmStarting) {
				//Vec2 P1 = step.dt * (m_force * m_linearJacobian.linear1 + (m_motorForce + m_limitForce) * m_motorJacobian.linear1);
				var P1X:Number = step.dt * (m_force * m_linearJacobian.linear1.x + (m_motorForce + m_limitForce) * m_motorJacobian.linear1.x);
				var P1Y:Number = step.dt * (m_force * m_linearJacobian.linear1.y + (m_motorForce + m_limitForce) * m_motorJacobian.linear1.y);
				//Vec2 P2 = step.dt * (m_force * m_linearJacobian.linear2 + (m_motorForce + m_limitForce) * m_motorJacobian.linear2);
				var P2X:Number = step.dt * (m_force * m_linearJacobian.linear2.x + (m_motorForce + m_limitForce) * m_motorJacobian.linear2.x);
				var P2Y:Number = step.dt * (m_force * m_linearJacobian.linear2.y + (m_motorForce + m_limitForce) * m_motorJacobian.linear2.y);
				//float32 L1 = step.dt * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
				var L1:Number = step.dt * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
				//float32 L2 = step.dt * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
				var L2:Number = step.dt * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
			
				//b1->m_linearVelocity += invMass1 * P1;
				b1.m_linearVelocity.x += invMass1 * P1X;
				b1.m_linearVelocity.y += invMass1 * P1Y;
				//b1->m_angularVelocity += invI1 * L1;
				b1.m_angularVelocity += invI1 * L1;
			
				//->m_linearVelocity += invMass2 * P2;
				b2.m_linearVelocity.x += invMass2 * P2X;
				b2.m_linearVelocity.y += invMass2 * P2Y;
				//->m_angularVelocity += invI2 * L2;
				b2.m_angularVelocity += invI2 * L2;
			}
		else {
				m_force = 0.0;
				m_torque = 0.0;
				m_limitForce = 0.0;
				m_motorForce = 0.0;
			}
		
			m_limitPositionImpulse = 0.0;
		}

		Internal override function solveVelocityConstraints(step:TimeStep):void {
			var b1:Body = m_body1;
			var b2:Body = m_body2;
		
			var invMass1:Number = b1.m_invMass;
			var invMass2:Number = b2.m_invMass;
			var invI1:Number = b1.m_invI;
			var invI2:Number = b2.m_invI;
		
			var oldLimitForce:Number;
		
			// Solve linear constraint.
			var linearCdot:Number = m_linearJacobian.compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);
			var force:Number = -step.inv_dt * m_linearMass * linearCdot;
			m_force += force;
		
			var P:Number = step.dt * force;
			//b1->m_linearVelocity += (invMass1 * P) * m_linearJacobian.linear1;
			b1.m_linearVelocity.x += (invMass1 * P) * m_linearJacobian.linear1.x;
			b1.m_linearVelocity.y += (invMass1 * P) * m_linearJacobian.linear1.y;
			//b1->m_angularVelocity += invI1 * P * m_linearJacobian.angular1;
			b1.m_angularVelocity += invI1 * P * m_linearJacobian.angular1;
		
			//->m_linearVelocity += (invMass2 * P) * m_linearJacobian.linear2;
			b2.m_linearVelocity.x += (invMass2 * P) * m_linearJacobian.linear2.x;
			b2.m_linearVelocity.y += (invMass2 * P) * m_linearJacobian.linear2.y;
			//.m_angularVelocity += invI2 * P * m_linearJacobian.angular2;
			b2.m_angularVelocity += invI2 * P * m_linearJacobian.angular2;
		
			// Solve angular constraint.
			var angularCdot:Number = b2.m_angularVelocity - b1.m_angularVelocity;
			var torque:Number = -step.inv_dt * m_angularMass * angularCdot;
			m_torque += torque;
		
			var L:Number = step.dt * torque;
			b1.m_angularVelocity -= invI1 * L;
			b2.m_angularVelocity += invI2 * L;
		
			// Solve linear motor constraint.
			if (m_enableMotor && m_limitState != e_equalLimits) {
				var motorCdot:Number = m_motorJacobian.compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity) - m_motorSpeed;
				var motorForce:Number = -step.inv_dt * m_motorMass * motorCdot;
				var oldMotorForce:Number = m_motorForce;
				m_motorForce = Box2dMath.clamp(m_motorForce + motorForce, -m_maxMotorForce, m_maxMotorForce);
				motorForce = m_motorForce - oldMotorForce;
			
				P = step.dt * motorForce;
				//b1.m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
				b1.m_linearVelocity.x += (invMass1 * P) * m_motorJacobian.linear1.x;
				b1.m_linearVelocity.y += (invMass1 * P) * m_motorJacobian.linear1.y;
				//b1.m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
				b1.m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
			
				//->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
				b2.m_linearVelocity.x += (invMass2 * P) * m_motorJacobian.linear2.x;
				b2.m_linearVelocity.y += (invMass2 * P) * m_motorJacobian.linear2.y;
				//->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
				b2.m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
			}
		
			// Solve linear limit constraint.
			if (m_enableLimit && m_limitState != e_inactiveLimit) {
				var limitCdot:Number = m_motorJacobian.compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);
				var limitForce:Number = -step.inv_dt * m_motorMass * limitCdot;
			
				if (m_limitState == e_equalLimits) {
					m_limitForce += limitForce;
				}
			else if (m_limitState == e_atLowerLimit) {
					oldLimitForce = m_limitForce;
					m_limitForce = Box2dMath.max(m_limitForce + limitForce, 0.0);
					limitForce = m_limitForce - oldLimitForce;
				}
			else if (m_limitState == e_atUpperLimit) {
					oldLimitForce = m_limitForce;
					m_limitForce = Box2dMath.min(m_limitForce + limitForce, 0.0);
					limitForce = m_limitForce - oldLimitForce;
				}
			
				P = step.dt * limitForce;
				//b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
				b1.m_linearVelocity.x += (invMass1 * P) * m_motorJacobian.linear1.x;
				b1.m_linearVelocity.y += (invMass1 * P) * m_motorJacobian.linear1.y;
				//b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
				b1.m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
			
				//->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
				b2.m_linearVelocity.x += (invMass2 * P) * m_motorJacobian.linear2.x;
				b2.m_linearVelocity.y += (invMass2 * P) * m_motorJacobian.linear2.y;
				//->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
				b2.m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
			}
		}

		Internal override function solvePositionConstraints():Boolean {
		
			var limitC:Number;
			var oldLimitImpulse:Number;
		
			var b1:Body = m_body1;
			var b2:Body = m_body2;
		
			var invMass1:Number = b1.m_invMass;
			var invMass2:Number = b2.m_invMass;
			var invI1:Number = b1.m_invI;
			var invI2:Number = b2.m_invI;
		
			var tMat:Mat22;
			var tX:Number;
		
			//Vec2 r1 = Mul(b1->m_xf.R, m_localAnchor1 - b1->GetLocalCenter());
			tMat = b1.m_xf.R;
			var r1X:Number = m_localAnchor1.x - b1.m_sweep.localCenter.x;
			var r1Y:Number = m_localAnchor1.y - b1.m_sweep.localCenter.y;
			tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
			r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
			r1X = tX;
			//Vec2 r2 = Mul(->m_xf.R, m_localAnchor2 - ->GetLocalCenter());
			tMat = b2.m_xf.R;
			var r2X:Number = m_localAnchor2.x - b2.m_sweep.localCenter.x;
			var r2Y:Number = m_localAnchor2.y - b2.m_sweep.localCenter.y;
			tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
			r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
			r2X = tX;
		
			//Vec2 p1 = b1->m_sweep.c + r1;
			var p1X:Number = b1.m_sweep.c.x + r1X;
			var p1Y:Number = b1.m_sweep.c.y + r1Y;
			//Vec2 p2 = ->m_sweep.c + r2;
			var p2X:Number = b2.m_sweep.c.x + r2X;
			var p2Y:Number = b2.m_sweep.c.y + r2Y;
			//Vec2 d = p2 - p1;
			var dX:Number = p2X - p1X;
			var dY:Number = p2Y - p1Y;
			//Vec2 ay1 = Mul(b1->m_xf.R, m_localYAxis1);
			tMat = b1.m_xf.R;
			var ay1X:Number = tMat.col1.x * m_localYAxis1.x + tMat.col2.x * m_localYAxis1.y;
			var ay1Y:Number = tMat.col1.y * m_localYAxis1.x + tMat.col2.y * m_localYAxis1.y;
		
			// Solve linear (point-to-line) constraint.
			//float32 linearC = Dot(ay1, d);
			var linearC:Number = ay1X * dX + ay1Y * dY;
			// Prevent overly large corrections.
			linearC = Box2dMath.clamp(linearC, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);
			var linearImpulse:Number = -m_linearMass * linearC;
		
			//b1->m_sweep.c += (invMass1 * linearImpulse) * m_linearJacobian.linear1;
			b1.m_sweep.c.x += (invMass1 * linearImpulse) * m_linearJacobian.linear1.x;
			b1.m_sweep.c.y += (invMass1 * linearImpulse) * m_linearJacobian.linear1.y;
			//b1->m_sweep.a += invI1 * linearImpulse * m_linearJacobian.angular1;
			b1.m_sweep.a += invI1 * linearImpulse * m_linearJacobian.angular1;
			//b1->SynchronizeTransform(); // updated by angular constraint
		
			//->m_sweep.c += (invMass2 * linearImpulse) * m_linearJacobian.linear2;
			b2.m_sweep.c.x += (invMass2 * linearImpulse) * m_linearJacobian.linear2.x;
			b2.m_sweep.c.y += (invMass2 * linearImpulse) * m_linearJacobian.linear2.y;
			//->m_sweep.a += invI2 * linearImpulse * m_linearJacobian.angular2;
			b2.m_sweep.a += invI2 * linearImpulse * m_linearJacobian.angular2;
			//->SynchronizeTransform(); // updated by angular constraint
			var positionError:Number = Box2dMath.abs(linearC);
		
			// Solve angular constraint.
			var angularC:Number = b2.m_sweep.a - b1.m_sweep.a - m_refAngle;
			// Prevent overly large corrections.
			angularC = Box2dMath.clamp(angularC, -Settings._maxAngularCorrection, Settings._maxAngularCorrection);
			var angularImpulse:Number = -m_angularMass * angularC;
		
			b1.m_sweep.a -= b1.m_invI * angularImpulse;
			b2.m_sweep.a += b2.m_invI * angularImpulse;
			b1.synchronizeTransform();
			b2.synchronizeTransform();
		
			var angularError:Number = Box2dMath.abs(angularC);
		
			// Solve linear limit constraint.
			if (m_enableLimit && m_limitState != e_inactiveLimit) {
			
				//Vec2 r1 = Mul(b1->m_xf.R, m_localAnchor1 - b1->GetLocalCenter());
				tMat = b1.m_xf.R;
				r1X = m_localAnchor1.x - b1.m_sweep.localCenter.x;
				r1Y = m_localAnchor1.y - b1.m_sweep.localCenter.y;
				tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
				r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
				r1X = tX;
				//Vec2 r2 = Mul(->m_xf.R, m_localAnchor2 - ->GetLocalCenter());
				tMat = b2.m_xf.R;
				r2X = m_localAnchor2.x - b2.m_sweep.localCenter.x;
				r2Y = m_localAnchor2.y - b2.m_sweep.localCenter.y;
				tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
				r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
				r2X = tX;
			
				//Vec2 p1 = b1->m_sweep.c + r1;
				p1X = b1.m_sweep.c.x + r1X;
				p1Y = b1.m_sweep.c.y + r1Y;
				//Vec2 p2 = ->m_sweep.c + r2;
				p2X = b2.m_sweep.c.x + r2X;
				p2Y = b2.m_sweep.c.y + r2Y;
				//Vec2 d = p2 - p1;
				dX = p2X - p1X;
				dY = p2Y - p1Y;
				//Vec2 ax1 = Mul(b1->m_xf.R, m_localXAxis1);
				tMat = b1.m_xf.R;
				var ax1X:Number = tMat.col1.x * m_localXAxis1.x + tMat.col2.x * m_localXAxis1.y;
				var ax1Y:Number = tMat.col1.y * m_localXAxis1.x + tMat.col2.y * m_localXAxis1.y;
			
				//float32 translation = Dot(ax1, d);
				var translation:Number = (ax1X * dX + ax1Y * dY);
				var limitImpulse:Number = 0.0;
			
				if (m_limitState == e_equalLimits) {
					// Prevent large angular corrections
					limitC = Box2dMath.clamp(translation, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);
					limitImpulse = -m_motorMass * limitC;
					positionError = Box2dMath.max(positionError, Box2dMath.abs(angularC));
				}
			else if (m_limitState == e_atLowerLimit) {
					limitC = translation - m_lowerTranslation;
					positionError = Box2dMath.max(positionError, -limitC);
				
					// Prevent large linear corrections and allow some slop.
					limitC = Box2dMath.clamp(limitC + Settings._linearSlop, -Settings._maxLinearCorrection, 0.0);
					limitImpulse = -m_motorMass * limitC;
					oldLimitImpulse = m_limitPositionImpulse;
					m_limitPositionImpulse = Box2dMath.max(m_limitPositionImpulse + limitImpulse, 0.0);
					limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
				}
			else if (m_limitState == e_atUpperLimit) {
					limitC = translation - m_upperTranslation;
					positionError = Box2dMath.max(positionError, limitC);
				
					// Prevent large linear corrections and allow some slop.
					limitC = Box2dMath.clamp(limitC - Settings._linearSlop, 0.0, Settings._maxLinearCorrection);
					limitImpulse = -m_motorMass * limitC;
					oldLimitImpulse = m_limitPositionImpulse;
					m_limitPositionImpulse = Box2dMath.min(m_limitPositionImpulse + limitImpulse, 0.0);
					limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
				}
			
				//b1->m_sweep.c += (invMass1 * limitImpulse) * m_motorJacobian.linear1;
				b1.m_sweep.c.x += (invMass1 * limitImpulse) * m_motorJacobian.linear1.x;
				b1.m_sweep.c.y += (invMass1 * limitImpulse) * m_motorJacobian.linear1.y;
				//b1->m_sweep.a += invI1 * limitImpulse * m_motorJacobian.angular1;
				b1.m_sweep.a += invI1 * limitImpulse * m_motorJacobian.angular1;
			
				//->m_sweep.c += (invMass2 * limitImpulse) * m_motorJacobian.linear2;
				b2.m_sweep.c.x += (invMass2 * limitImpulse) * m_motorJacobian.linear2.x;
				b2.m_sweep.c.y += (invMass2 * limitImpulse) * m_motorJacobian.linear2.y;
				//->m_sweep.a += invI2 * limitImpulse * m_motorJacobian.angular2;
				b2.m_sweep.a += invI2 * limitImpulse * m_motorJacobian.angular2;
			
				b1.synchronizeTransform();
				b2.synchronizeTransform();
			}
		
			return positionError <= Settings._linearSlop && angularError <= Settings._angularSlop;
		}

		public var m_localAnchor1:Vec2 = new Vec2();
		public var m_localAnchor2:Vec2 = new Vec2();
		public var m_localXAxis1:Vec2 = new Vec2();
		public var m_localYAxis1:Vec2 = new Vec2();
		public var m_refAngle:Number;
		public var m_linearJacobian:Jacobian = new Jacobian();
		public var m_linearMass:Number;				
		// effective mass for point-to-line constraint.
		public var m_force:Number;
		public var m_angularMass:Number;			
		// effective mass for angular constraint.
		public var m_torque:Number;
		public var m_motorJacobian:Jacobian = new Jacobian();
		public var m_motorMass:Number;			
		// effective mass for motor/limit translational constraint.
		public var m_motorForce:Number;
		public var m_limitForce:Number;
		public var m_limitPositionImpulse:Number;
		public var m_lowerTranslation:Number;
		public var m_upperTranslation:Number;
		public var m_maxMotorForce:Number;
		public var m_motorSpeed:Number;
		public var m_enableLimit:Boolean;
		public var m_enableMotor:Boolean;
		public var m_limitState:int;
	}
}
